/**
 * @file	electronic_throttle.cpp
 * @brief	Electronic Throttle driver
 *
 * @see test test_etb.cpp
 *
 * PPS=pedal position sensor=AcceleratorPedal
 * TPS=throttle position sensor, this one is inside ETB=electronic throttle body
 *
 * Limited user documentation at https://wiki.rusefi.com/HOWTO_electronic_throttle_body
 *
 *
 *  ETB is controlled according to pedal position input (pedal position sensor is a potentiometer)
 *    pedal 0% means pedal not pressed / idle
 *    pedal 100% means pedal all the way down
 *  (not TPS - not the one you can calibrate in TunerStudio)
 *
 *
 * See also pid.cpp
 *
 * @date Dec 7, 2013
 * @author Andrey Belomutskiy, (c) 2012-2020
 *
 * This file is part of rusEfi - see http://rusefi.com
 *
 * rusEfi is free software; you can redistribute it and/or modify it under the terms of
 * the GNU General Public License as published by the Free Software Foundation; either
 * version 3 of the License, or (at your option) any later version.
 *
 * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
 * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include "pch.h"

#include "electronic_throttle_impl.h"

#if EFI_ELECTRONIC_THROTTLE_BODY

#include "dc_motor.h"
#include "dc_motors.h"
#include "defaults.h"
#include "tunerstudio.h"
#include "tunerstudio_calibration_channel.h"
#include "transition_events.h"

#if defined(HAS_OS_ACCESS)
#error "Unexpected OS ACCESS HERE"
#endif

#if HW_PROTEUS
#include "proteus_meta.h"
#endif // HW_PROTEUS

#ifndef ETB_MAX_COUNT
#define ETB_MAX_COUNT 2
#endif /* ETB_MAX_COUNT */

#ifndef ETB_INTERMITTENT_LIMIT
#define ETB_INTERMITTENT_LIMIT 50
#endif

static pedal2tps_t pedal2tpsMap{"p2t"};
static Map3D<ETB2_TRIM_RPM_SIZE, ETB2_TRIM_SIZE, int8_t, uint8_t, uint8_t> throttle2TrimTable{"t2t"};
static Map3D<TRACTION_CONTROL_ETB_DROP_SLIP_SIZE, TRACTION_CONTROL_ETB_DROP_SPEED_SIZE, int8_t, uint16_t, uint8_t> tcEtbDropTable{"tce"};

constexpr float etbPeriodSeconds = 1.0f / ETB_LOOP_FREQUENCY;

//static bool startupPositionError = false;

//#define STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD 5

static const float hardCodedetbHitachiBiasBins[8] = {0.0, 19.0, 21.0, 22.0, 23.0, 25.0, 30.0, 100.0};

static const float hardCodedetbHitachiBiasValues[8] = {-18.0, -17.0, -15.0, 0.0, 16.0, 20.0, 20.0, 20.0};

/* Generated by TS2C on Thu Aug 20 21:10:02 EDT 2020*/
void setHitachiEtbBiasBins() {
	copyArray(config->etbBiasBins, hardCodedetbHitachiBiasBins);
	copyArray(config->etbBiasValues, hardCodedetbHitachiBiasValues);
}

static SensorType functionToPositionSensor(dc_function_e func) {
	switch(func) {
		case DC_Throttle1: return SensorType::Tps1;
		case DC_Throttle2: return SensorType::Tps2;
		case DC_IdleValve: return SensorType::IdlePosition;
		case DC_Wastegate: return SensorType::WastegatePosition;
		default: return SensorType::Invalid;
	}
}

static SensorType functionToTpsSensor(dc_function_e func) {
	switch(func) {
		case DC_Throttle1: return SensorType::Tps1;
		case DC_Throttle2: return SensorType::Tps2;
		case DC_IdleValve: return SensorType::IdlePosition;
		case DC_Wastegate: return SensorType::WastegatePosition;
		default: return SensorType::Invalid;
	}
}

static SensorType functionToTpsSensorPrimary(dc_function_e func) {
	switch(func) {
		case DC_Throttle1: return SensorType::Tps1Primary;
		case DC_Throttle2: return SensorType::Tps2Primary;
		case DC_IdleValve: return SensorType::IdlePosition;
		case DC_Wastegate: return SensorType::WastegatePosition;
		default: return SensorType::Invalid;
	}
}

static SensorType functionToTpsSensorSecondary(dc_function_e func) {
	switch(func) {
		case DC_Throttle1: return SensorType::Tps1Secondary;
		case DC_Throttle2: return SensorType::Tps2Secondary;
		/* No secondary sensors for Idle and EWG */
		default: return SensorType::Invalid;
	}
}

#if EFI_TUNER_STUDIO
static TsCalMode functionToCalModePriMin(dc_function_e func) {
	switch (func) {
		case DC_Throttle1: return TsCalMode::Tps1Min;
		case DC_Throttle2: return TsCalMode::Tps2Min;
		case DC_Wastegate: return TsCalMode::EwgPosMin;
		default: return TsCalMode::None;
	}
}

static TsCalMode functionToCalModePriMax(dc_function_e func) {
	switch (func) {
		case DC_Throttle1: return TsCalMode::Tps1Max;
		case DC_Throttle2: return TsCalMode::Tps2Max;
		case DC_Wastegate: return TsCalMode::EwgPosMax;
		default: return TsCalMode::None;
	}
}

static TsCalMode functionToCalModeSecMin(dc_function_e func) {
	switch (func) {
		case DC_Throttle1: return TsCalMode::Tps1SecondaryMin;
		case DC_Throttle2: return TsCalMode::Tps2SecondaryMin;
		default: return TsCalMode::None;
	}
}

static TsCalMode functionToCalModeSecMax(dc_function_e func) {
	switch (func) {
		case DC_Throttle1: return TsCalMode::Tps1SecondaryMax;
		case DC_Throttle2: return TsCalMode::Tps2SecondaryMax;
		default: return TsCalMode::None;
	}
}
#endif // EFI_TUNER_STUDIO

#define ETB_DUTY_LIMIT 0.9
// this macro clamps both positive and negative percentages from about -100% to 100%
#define ETB_PERCENT_TO_DUTY(x) (clampF(-ETB_DUTY_LIMIT, 0.01f * (x), ETB_DUTY_LIMIT))

PUBLIC_API_WEAK bool isBoardAllowingLackOfPps() {
  return false;
}

bool EtbController::init(dc_function_e function, DcMotor *motor, pid_s *pidParameters, const ValueProvider3D* pedalProvider) {
	state = (uint8_t)EtbState::InInit;
	if (function == DC_None) {
		// if not configured, don't init.
		state = (uint8_t)EtbState::NotEbt;
		etbErrorCode = (int8_t)EtbStatus::NotConfigured;
		return false;
	}

	m_function = function;
	m_positionSensor = functionToPositionSensor(function);

	// If we are a throttle, require redundant TPS sensor
	if (isEtbMode()) {
		// If no sensor is configured for this throttle, skip initialization.
		if (!Sensor::hasSensor(functionToTpsSensor(function))) {
			etbErrorCode = (int8_t)EtbStatus::TpsError;
			return false;
		}

		if (!isBoardAllowingLackOfPps() && !Sensor::isRedundant(m_positionSensor)) {
			etbErrorCode = (int8_t)EtbStatus::Redundancy;
			return false;
		}
	}

	m_motor = motor;
	m_pedalProvider = pedalProvider;

	m_pid.initPidClass(pidParameters);

#if !EFI_UNIT_TEST
	if (isEtbMode()) {
		m_pid.iTermMin = engineConfiguration->etb_iTermMin;
		m_pid.iTermMax = engineConfiguration->etb_iTermMax;
	} else {
		// Some defaults from setDefaultEtbParameters(), find better values for EWG and Idle or add config options
		m_pid.iTermMin = -30;
		m_pid.iTermMax = 30;
	}
#endif

	// Ignore 3% position error before complaining
	m_targetErrorAccumulator.init(3.0f, etbPeriodSeconds);

	state = (uint8_t)EtbState::SuccessfulInit;
	return true;
}

#if EFI_UNIT_TEST
int ebtResetCounter;
#endif // EFI_UNIT_TEST

void EtbController::reset(const char *reason) {
	efiPrintf("ETB reset %s", reason);
	m_shouldResetPid = true;
	etbTpsErrorCounter = 0;
	etbPpsErrorCounter = 0;
#if EFI_UNIT_TEST
	ebtResetCounter++;
#endif // EFI_UNIT_TEST

}

// todo: document why is EtbController not engine_module?
void EtbController::onConfigurationChange(pid_s* previousConfiguration) {
	if (m_motor && !m_pid.isSame(previousConfiguration)) {
	  efiPrintf(" ETB m_shouldResetPid");
		m_shouldResetPid = true;
	}

	doInitElectronicThrottle(/*isStartupInit*/false);
}

void EtbController::showStatus() {
	m_pid.showPidStatus("ETB");
}

expected<percent_t> EtbController::observePlant() {
	expected<percent_t> plant = Sensor::get(m_positionSensor);
	validPlantPosition = plant.Valid;
	return plant;
}

void EtbController::setIdlePosition(percent_t pos) {
	m_idlePosition = pos;
}

void EtbController::setWastegatePosition(percent_t pos) {
	m_wastegatePosition = pos;
}

expected<percent_t> EtbController::getSetpoint() {
	switch (m_function) {
		case DC_Throttle1:
		case DC_Throttle2:
			return getSetpointEtb();
		case DC_IdleValve:
			return getSetpointIdleValve();
		case DC_Wastegate:
			return getSetpointWastegate();
		default:
			return unexpected;
	}
}

expected<percent_t> EtbController::getSetpointIdleValve() const {
	// VW ETB idle mode uses an ETB only for idle (a mini-ETB sets the lower stop, and a normal cable
	// can pull the throttle up off the stop.), so we directly control the throttle with the idle position.
#if EFI_TUNER_STUDIO && (EFI_PROD_CODE || EFI_SIMULATOR)
	// todo: where do we want to log this? engine->outputChannels.etbTarget = m_idlePosition;
#endif // EFI_TUNER_STUDIO
	return clampPercentValue(m_idlePosition);
}

expected<percent_t> EtbController::getSetpointWastegate() const {
	percent_t targetPosition = m_wastegatePosition + getLuaAdjustment();

	return clampPercentValue(targetPosition);
}

float getSanitizedPedal() {
	auto pedalPosition = Sensor::get(SensorType::AcceleratorPedal);
	// If the pedal has failed, just use 0 position.
	// This is safer than disabling throttle control - we can at least push the throttle closed
	// and let the engine idle.
	return clampPercentValue(pedalPosition.value_or(0));
}

PUBLIC_API_WEAK float boardAdjustEtbTarget(float currentEtbTarget) {
  return currentEtbTarget;
}

expected<percent_t> EtbController::getSetpointEtb() {
	// Autotune runs with 50% target position
	if (m_isAutotune) {
		return 50.0f;
	}

//	// A few extra preconditions if throttle control is invalid
//	if (startupPositionError) {
//		return unexpected;
//	}

	// If the pedal map hasn't been set, we can't provide a setpoint.
	if (!m_pedalProvider) {
    state = (uint8_t)EtbState::NoPedal;
		return unexpected;
	}

	float sanitizedPedal = getSanitizedPedal();
	float rpm = Sensor::getOrZero(SensorType::Rpm);

	percent_t preBoard = m_pedalProvider->getValue(rpm, sanitizedPedal);
	etbCurrentTarget = boardAdjustEtbTarget(preBoard);
	boardEtbAdjustment = etbCurrentTarget - preBoard;

	percent_t etbIdlePosition = clampPercentValue(m_idlePosition);
	percent_t etbIdleAddition = PERCENT_DIV * engineConfiguration->etbIdleThrottleRange * etbIdlePosition;

	// Interpolate so that the idle adder just "compresses" the throttle's range upward.
	// [0, 100] -> [idle, 100]
	// 0% target from table -> idle position as target
	// 100% target from table -> 100% target position
	targetWithIdlePosition = interpolateClamped(0, etbIdleAddition, 100, 100, etbCurrentTarget);

	percent_t targetPosition = targetWithIdlePosition + getLuaAdjustment();
	// just an additional logging data point
	adjustedEtbTarget = targetPosition;

#if EFI_ANTILAG_SYSTEM
	if (engine->antilagController.isAntilagCondition) {
		targetPosition += engineConfiguration->ALSEtbAdd;
	}
#endif /* EFI_ANTILAG_SYSTEM */

  float vehicleSpeed = Sensor::getOrZero(SensorType::VehicleSpeed);
  float wheelSlip = Sensor::getOrZero(SensorType::WheelSlipRatio);
  tcEtbDrop = tcEtbDropTable.getValue(wheelSlip, vehicleSpeed);

	// Apply any adjustment that this throttle alone needs
	// Clamped to +-10 to prevent anything too wild
	trim = clampF(-10, getThrottleTrim(rpm, targetPosition), 10);
	targetPosition += trim + tcEtbDrop;

	// Clamp before rev limiter to avoid ineffective rev limit due to crazy out of range position target
	targetPosition = clampPercentValue(targetPosition);

	// Lastly, apply ETB rev limiter
	auto etbRpmLimit = engineConfiguration->etbRevLimitStart;
	if (etbRpmLimit != 0) {
		auto fullyLimitedRpm = etbRpmLimit + engineConfiguration->etbRevLimitRange;

		float targetPositionBefore = targetPosition;
		// Linearly taper throttle to closed from the limit across the range
		targetPosition = interpolateClamped(etbRpmLimit, targetPosition, fullyLimitedRpm, 0, rpm);

		// rev limit active if the position was changed by rev limiter
		etbRevLimitActive = std::abs(targetPosition - targetPositionBefore) > 0.1f;
	}

	float minPosition = engineConfiguration->etbMinimumPosition;

	// Keep the throttle just barely off the lower stop, and less than the user-configured maximum
	float maxPosition = engineConfiguration->etbMaximumPosition;
	// Don't allow max position over 100
	maxPosition = std::min(maxPosition, 100.0f);

	targetPosition = clampF(minPosition, targetPosition, maxPosition);
	m_adjustedTarget = targetPosition;

	return targetPosition;
}

void EtbController::setLuaAdjustment(float adjustment) {
	luaAdjustment = adjustment;
	m_luaAdjustmentTimer.reset();
}

/**
 * positive adjustment opens TPS, negative closes TPS
 */
float EtbController::getLuaAdjustment() const {
	// If the lua position hasn't been set in 0.2 second, don't adjust!
	// This avoids a stuck throttle due to hung/rogue/etc Lua script
	if (m_luaAdjustmentTimer.getElapsedSeconds() > 0.2f) {
		return 0;
	} else {
		return luaAdjustment;
	}
}

percent_t EtbController2::getThrottleTrim(float rpm, percent_t targetPosition) const {
	return m_throttle2Trim.getValue(rpm, targetPosition);
}

expected<percent_t> EtbController::getOpenLoop(percent_t target) {
	// Don't apply open loop for idle valve, only real ETB or wastegate
	switch(m_function){
		case DC_Throttle1:
		case DC_Throttle2: {
			etbFeedForward = interpolate2d(target, config->etbBiasBins, config->etbBiasValues);
			break;
		}
		case DC_Wastegate: {
			etbFeedForward = interpolate2d(target, config->dcWastegateBiasBins, config->dcWastegateBiasValues);
			break;
		}
		case DC_IdleValve: {
			etbFeedForward = 0;
			break;
		}
		default: { // or DC_None
			etbFeedForward = 0;
		}
	}

	return etbFeedForward;
}

expected<percent_t> EtbController::getClosedLoopAutotune(percent_t target, percent_t actualThrottlePosition) {
	// Estimate gain at current position - this should be well away from the spring and in the linear region
	// GetSetpoint sets this to 50%
	bool isPositive = actualThrottlePosition > target;

	float autotuneAmplitude = 20;

	// End of cycle - record & reset
	if (!isPositive && m_lastIsPositive) {
		// Determine period
		float tu = m_cycleTimer.getElapsedSecondsAndReset(getTimeNowNt());

		// Determine amplitude
		float a = m_maxCycleTps - m_minCycleTps;

		// Filter - it's pretty noisy since the ultimate period is not very many loop periods
		constexpr float alpha = 0.05;
		m_a  = alpha * a  + (1 - alpha) * m_a;
		m_tu = alpha * tu + (1 - alpha) * m_tu;

		// Reset bounds
		m_minCycleTps = 100;
		m_maxCycleTps = 0;

		// Math is for Åström–Hägglund (relay) auto tuning
		// https://warwick.ac.uk/fac/cross_fac/iatl/reinvention/archive/volume5issue2/hornsey

		// Amplitude of input (duty cycle %)
		float b = 2 * autotuneAmplitude;

		// Ultimate gain per A-H relay tuning rule
		float ku = 4 * b / (CONST_PI * m_a);

		// The multipliers below are somewhere near the "no overshoot"
		// and "some overshoot" flavors of the Ziegler-Nichols method
		// Kp
		float kp = 0.35f * ku;
		float ki = 0.25f * ku / m_tu;
		float kd = 0.08f * ku * m_tu;

		// Publish to TS state
#if EFI_TUNER_STUDIO
		// Every 5 cycles (of the throttle), cycle to the next value
		if (m_autotuneCounter >= 5) {
			m_autotuneCounter = 0;
			m_autotuneCurrentParam = (m_autotuneCurrentParam + 1) % 3; // three ETB calibs: P-I-D
		}

		m_autotuneCounter++;

		switch (m_autotuneCurrentParam) {
		case 0:
			tsCalibrationSetData(TsCalMode::EtbKp, kp);
			break;
		case 1:
			tsCalibrationSetData(TsCalMode::EtbKi, ki);
			break;
		case 2:
			tsCalibrationSetData(TsCalMode::EtbKd, kd);
			break;
		}

		// Also output to debug channels if configured
		if (engineConfiguration->debugMode == DBG_ETB_AUTOTUNE) {
			// a - amplitude of output (TPS %)
			engine->outputChannels.debugFloatField1 = m_a;
			// b - amplitude of input (Duty cycle %)
			engine->outputChannels.debugFloatField2 = b;
			// Tu - oscillation period (seconds)
			engine->outputChannels.debugFloatField3 = m_tu;

			engine->outputChannels.debugFloatField4 = ku;
			engine->outputChannels.debugFloatField5 = kp;
			engine->outputChannels.debugFloatField6 = ki;
			engine->outputChannels.debugFloatField7 = kd;
		}
#endif
		// TODO: directly update PID settings in engineConfiguration
	}

	m_lastIsPositive = isPositive;

	// Find the min/max of each cycle
	if (actualThrottlePosition < m_minCycleTps) {
		m_minCycleTps = actualThrottlePosition;
	}

	if (actualThrottlePosition > m_maxCycleTps) {
		m_maxCycleTps = actualThrottlePosition;
	}

	// Bang-bang control the output to induce oscillation
	return autotuneAmplitude * (isPositive ? -1 : 1);
}

expected<percent_t> EtbController::getClosedLoop(percent_t target, percent_t observation) {
	if (m_shouldResetPid) {
		m_pid.reset();
		onTransitionEvent(TransitionEvent::EtbPidReset);
		m_shouldResetPid = false;
	}

	if (m_isAutotune) {
		state = (uint8_t)EtbState::Autotune;

		m_targetErrorAccumulator.reset();

		return getClosedLoopAutotune(target, observation);
	} else {
		checkJam(target, observation);

		integralError = m_targetErrorAccumulator.accumulate(target - observation);

		float dt = m_cycleTimer.getElapsedSecondsAndReset(getTimeNowNt());
		m_lastPidDtMs = dt * 1000.0;

		// Normal case - use PID to compute closed loop part
		return m_pid.getOutput(target, observation, dt);
	}
}

void EtbController::setOutput(expected<percent_t> outputValue) {
#if EFI_TUNER_STUDIO
	// Only report first-throttle stats
	if (m_function == DC_Throttle1) {
		engine->outputChannels.etb1DutyCycle = outputValue.value_or(0);
	}
#endif

	if (!m_motor) {
		state = (uint8_t)EtbState::NoMotor;
		return;
	}

	bool isEnabled;
	if (!isEtbMode()) {
	  // technical debt: non-ETB usages of DC motor are still mixed into ETB controller?
		state = (uint8_t)EtbState::NotEbt;
		isEnabled = true;
	} else if (!getLimpManager()->allowElectronicThrottle()) {
		state = (uint8_t)EtbState::LimpProhibited;
		isEnabled = false;
	} else if (engineConfiguration->pauseEtbControl) {
		state = (uint8_t)EtbState::Paused;
		isEnabled = false;
	} else if (!outputValue) {
		state = (uint8_t)EtbState::NoOutput;
		isEnabled = false;
	} else {
		state = (uint8_t)EtbState::Active;
		isEnabled = true;
	}

	// If not ETB, or ETB is allowed, output is valid, and we aren't paused, output to motor.
	if (isEnabled) {
		m_motor->enable();
		m_motor->set(ETB_PERCENT_TO_DUTY(outputValue.Value));
	} else {
		// Otherwise disable the motor.
		m_motor->disable("no-ETB");
	}
}

bool EtbController::checkStatus() {
#if EFI_TUNER_STUDIO
	// Only debug throttle #1
	if (m_function == DC_Throttle1) {
		m_pid.postState(engine->outputChannels.etbStatus);
	} else if (m_function == DC_Wastegate) {
		m_pid.postState(engine->outputChannels.wastegateDcStatus);
	}
#endif /* EFI_TUNER_STUDIO */

	if (!isEtbMode()) {
		// no validation for h-bridge or idle mode
		return true;
	}
	// ETB-specific code belo. The whole mix-up between DC and ETB is shameful :(

	// Only allow autotune with stopped engine, and on the first throttle
	// Update local state about autotune
	m_isAutotune = Sensor::getOrZero(SensorType::Rpm) == 0
		&& engine->etbAutoTune
		&& m_function == DC_Throttle1;

	bool shouldCheckSensorFunction = engine->module<SensorChecker>()->analogSensorsShouldWork();

	if (!m_isAutotune && shouldCheckSensorFunction) {
		bool isTpsError = !Sensor::get(m_positionSensor).Valid;

		// If we have an error that's new, increment the counter
		if (isTpsError && !hadTpsError) {
			etbTpsErrorCounter++;
		}

		hadTpsError = isTpsError;

		bool isPpsError = !Sensor::get(SensorType::AcceleratorPedal).Valid;

		// If we have an error that's new, increment the counter
		if (isPpsError && !hadPpsError) {
			etbPpsErrorCounter++;
		}

		hadPpsError = isPpsError;
	} else {
		// Either sensors are expected to not work, or autotune is running, so reset the error counter
		etbTpsErrorCounter = 0;
		etbPpsErrorCounter = 0;
	}

	EtbStatus localReason = EtbStatus::None;
	if (etbTpsErrorCounter > ETB_INTERMITTENT_LIMIT) {
		localReason = EtbStatus::IntermittentTps;
#if EFI_SHAFT_POSITION_INPUT
	} else if (engineConfiguration->disableEtbWhenEngineStopped
	  && !engine->triggerCentral.engineMovedRecently()
	  && !engine->etbAutoTune) {
		localReason = EtbStatus::EngineStopped;
#endif // EFI_SHAFT_POSITION_INPUT
	} else if (etbPpsErrorCounter > ETB_INTERMITTENT_LIMIT) {
		localReason = EtbStatus::IntermittentPps;
	} else if (engine->engineState.lua.luaDisableEtb) {
		localReason = EtbStatus::Lua;
	} else if (!getLimpManager()->allowElectronicThrottle()) {
		localReason = EtbStatus::JamDetected;
	} else if(!isBoardAllowingLackOfPps() && !Sensor::isRedundant(SensorType::AcceleratorPedal)) {
		localReason = EtbStatus::Redundancy;
	}

	etbErrorCode = (int8_t)localReason;

	return localReason == EtbStatus::None;
}

void EtbController::update() {
#if !EFI_UNIT_TEST
	// If we didn't get initialized, fail fast
	if (!m_motor) {
		state = (uint8_t)EtbState::FailFast;
		return;
	}
#endif // EFI_UNIT_TEST

	bool isOk = checkStatus();

	if (!isOk) {
		// If engine is stopped and so configured, skip the ETB update entirely
		// This is quieter and pulls less power than leaving it on all the time
		m_motor->disable("etb status");
		return;
	}

	ClosedLoopController::update();

	if (isEtbMode() && !validPlantPosition) {
		etbErrorCode = (int8_t)EtbStatus::TpsError;
	}
}

void EtbController::checkJam(percent_t setpoint, percent_t observation) {
	float absError = std::abs(setpoint - observation);

	auto jamDetectThreshold = engineConfiguration->etbJamDetectThreshold;
	auto jamTimeout = engineConfiguration->etbJamTimeout;

	if (jamDetectThreshold != 0 && jamTimeout != 0) {
		auto nowNt = getTimeNowNt();

		if (absError > jamDetectThreshold && engine->module<IgnitionController>()->getIgnState()) {
			if (m_jamDetectTimer.hasElapsedSec(jamTimeout)) {
				efiPrintf(" ************* ETB is jammed! ***************");
				jamDetected = true;

				getLimpManager()->reportEtbProblem();
			}
		} else {
			m_jamDetectTimer.reset(nowNt);
			jamDetected = false;
		}

		jamTimer = m_jamDetectTimer.getElapsedSeconds(nowNt);
	}
}

#if !EFI_UNIT_TEST
/**
 * Things running on a timer (instead of a thread) don't participate it the RTOS's thread priority system,
 * and operate essentially "first come first serve", which risks starvation.
 * Since ETB is a safety critical device, we need the hard RTOS guarantee that it will be scheduled over other less important tasks.
 */
#include "periodic_thread_controller.h"
#endif // EFI_UNIT_TEST

#include <utility>

// real implementation (we mock for some unit tests)
EtbImpl<EtbController1> etb1;
EtbImpl<EtbController2> etb2(throttle2TrimTable);

static EtbController* etbControllers[] = { &etb1, &etb2 };
static_assert(ETB_COUNT == sizeof(etbControllers) / sizeof(EtbController*));

void blinkEtbErrorCodes(bool blinkPhase) {
	for (int i = 0;i<ETB_COUNT;i++) {
		int8_t etbErrorCode = etbControllers[i]->etbErrorCode;
		if (etbErrorCode && engine->etbAutoTune) {
			etbErrorCode = (int8_t)EtbStatus::AutoTune;
		}
		etbControllers[i]->etbErrorCodeBlinker = blinkPhase ? 0 : etbErrorCode;
	}
}

#if !EFI_UNIT_TEST

struct DcThread final : public PeriodicController<512> {
	DcThread() : PeriodicController("DC", PRIO_ETB, ETB_LOOP_FREQUENCY) {}

	void PeriodicTask(efitick_t) override {
		// Simply update all controllers
		for (int i = 0 ; i < ETB_COUNT; i++) {
			auto controller = engine->etbControllers[i];
			assertNotNullVoid(controller);
			etbControllers[i]->update();
		}
	}
};

static DcThread dcThread CCM_OPTIONAL;

#endif // !EFI_UNIT_TEST

#if EFI_UNIT_TEST
void etbPidReset() {
	for (int i = 0 ; i < ETB_COUNT; i++) {
		if (auto controller = engine->etbControllers[i]) {
			assertNotNullVoid(controller);
			controller->reset("unit_test");
		}
	}
	ebtResetCounter = 0;
}
#endif // EFI_UNIT_TEST

void etbAutocal(dc_function_e function, bool reportToTs) {
	for (size_t i = 0 ; i < ETB_COUNT; i++) {
		/* TODO: use from engine, add getFunction() to base class */
		//if (auto controller = engine->etbControllers[i]) {
		if (auto controller = etbControllers[i]) {
			assertNotNullVoid(controller);
			if (controller->getFunction() == function) {
				/* TODO: is it possible that we have several controllers with same function? */
				controller->autoCalibrateTps(reportToTs);
				// todo fix root cause! work-around: make sure not to write bad tune since that would brick requestBurn();
			}
		}
	}
}

EtbStatus etbGetState(size_t throttleIndex)
{
	if (throttleIndex >= ETB_COUNT) {
		return EtbStatus::NotConfigured;
	}

	return (EtbStatus)etbControllers[throttleIndex]->etbErrorCode;
}

/**
 * This specific throttle has default position of about 7% open
 */
static const float boschBiasBins[] = {
	0, 1, 5, 7, 14, 65, 66, 100
};
static const float boschBiasValues[] = {
	-15, -15, -10, 0, 19, 20, 26, 28
};

void setBoschVAGETB() {
	engineConfiguration->tpsMin = 890; // convert 12to10 bit (ADC/4)
	engineConfiguration->tpsMax = 70; // convert 12to10 bit (ADC/4)

	engineConfiguration->tps1SecondaryMin = 102;
	engineConfiguration->tps1SecondaryMax = 891;

	engineConfiguration->etb.pFactor = 5.12;
	engineConfiguration->etb.iFactor = 47;
	engineConfiguration->etb.dFactor = 0.088;
	engineConfiguration->etb.offset = 0;
}

void setBoschVNH2SP30Curve() {
	copyArray(config->etbBiasBins, boschBiasBins);
	copyArray(config->etbBiasValues, boschBiasValues);
}

void setDefaultEtbParameters() {
	engineConfiguration->etbIdleThrottleRange = 15;

	setLinearCurve(config->pedalToTpsPedalBins, /*from*/0, /*to*/100, 1);
	setRpmTableBin(config->pedalToTpsRpmBins);

	for (int pedalIndex = 0;pedalIndex<PEDAL_TO_TPS_SIZE;pedalIndex++) {
		for (int rpmIndex = 0;rpmIndex<PEDAL_TO_TPS_RPM_SIZE;rpmIndex++) {
			config->pedalToTpsTable[pedalIndex][rpmIndex] = config->pedalToTpsPedalBins[pedalIndex];
		}
	}

	// Default is to run each throttle off its respective hbridge
	engineConfiguration->etbFunctions[0] = DC_Throttle1;
	engineConfiguration->etbFunctions[1] = DC_Throttle2;

	engineConfiguration->etbFreq = DEFAULT_ETB_PWM_FREQUENCY;

	// voltage, not ADC like with TPS
	setPPSCalibration(0, 5, 5, 0);

	engineConfiguration->etb = {
		1,		// Kp
		10,		// Ki
		0.05,	// Kd
		0,		// offset
		0,		// Update rate, unused
		-100, 100 // min/max
	};

	engineConfiguration->etb_iTermMin = -30;
	engineConfiguration->etb_iTermMax = 30;

	engineConfiguration->etbJamDetectThreshold = 10;
//	engineConfiguration->etbJamTimeout = 1;
}

void onConfigurationChangeElectronicThrottleCallback(engine_configuration_s *previousConfiguration) {
	for (int i = 0; i < ETB_COUNT; i++) {
		etbControllers[i]->onConfigurationChange(&previousConfiguration->etb);
	}
}

static const float defaultBiasBins[] = {
	0, 1, 2, 4, 7, 98, 99, 100
};
static const float defaultBiasValues[] = {
	-20, -18, -17, 0, 20, 21, 22, 25
};

void setDefaultEtbBiasCurve() {
	copyArray(config->etbBiasBins, defaultBiasBins);
	copyArray(config->etbBiasValues, defaultBiasValues);
	copyArray(config->dcWastegateBiasBins, defaultBiasBins);
	copyArray(config->dcWastegateBiasValues, defaultBiasValues);
}

void unregisterEtbPins() {
	// todo: we probably need an implementation here?!
}

static pid_s* getPidForDcFunction(dc_function_e function) {
	switch (function) {
		case DC_Wastegate: return &engineConfiguration->etbWastegatePid;
		default: return &engineConfiguration->etb;
	}
}

PUBLIC_API_WEAK ValueProvider3D* pedal2TpsProvider() {
  return &pedal2tpsMap;
}

void doInitElectronicThrottle(bool isStartupInit) {
	bool anyEtbConfigured = false;

	// todo: technical debt: we still have DC motor code initialization in ETB-specific file while DC motors are used not just as ETB
	// like DC motor wastegate code flow should probably NOT go through electronic_throttle.cpp right?
	// todo: rename etbFunctions to something-without-etb for same reason?
	for (int i = 0 ; i < ETB_COUNT; i++) {
		auto func = engineConfiguration->etbFunctions[i];
		if (func == DC_None) {
			// do not touch HW pins if function not selected, this way Lua can use DC motor hardware pins directly
			continue;
		}
		auto motor = initDcMotor("ETB disable",
				engineConfiguration->etbIo[i], i, engineConfiguration->etb_use_two_wires);

		auto controller = engine->etbControllers[i];
		criticalAssertVoid(controller != nullptr, "null ETB");

		auto pid = getPidForDcFunction(func);

		bool dcConfigured = controller->init(func, motor, pid, pedal2TpsProvider());
		if (isStartupInit && dcConfigured) {
			controller->reset("init");
		}
		anyEtbConfigured |= dcConfigured && controller->isEtbMode();
	}

	// It's not valid to have a PPS without any ETBs - check that at least one ETB was enabled along with the pedal
	if (!anyEtbConfigured && Sensor::hasSensor(SensorType::AcceleratorPedalPrimary)) {
		criticalError("A pedal position sensor was configured, but no electronic throttles are configured.");
	}

#if 0 && ! EFI_UNIT_TEST
	percent_t startupThrottlePosition = getTPS();
	if (std::abs(startupThrottlePosition - engineConfiguration->etbNeutralPosition) > STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD) {
		/**
		 * Unexpected electronic throttle start-up position is worth a critical error
		 */
		firmwareError(ObdCode::OBD_Throttle_Actuator_Control_Range_Performance_Bank_1, "startup ETB position %.2f not %d",
				startupThrottlePosition,
				engineConfiguration->etbNeutralPosition);
		startupPositionError = true;
	}
#endif /* EFI_UNIT_TEST */

#if !EFI_UNIT_TEST
	static bool started = false;
	if (started == false) {
		dcThread.start();
		started = true;
	}
#endif
}

void initElectronicThrottle() {
	if (hasFirmwareError()) {
		return;
	}

	for (int i = 0; i < ETB_COUNT; i++) {
		engine->etbControllers[i] = etbControllers[i];
	}

#if EFI_PROD_CODE
	addConsoleAction("etbautocal", [](){
		efiPrintf("etbAutocal invoked");
		etbAutocal(DC_Throttle1);
	});

	addConsoleAction("etbinfo", [](){
		efiPrintf("etbAutoTune=%d", engine->etbAutoTune);
		efiPrintf("TPS=%.2f", Sensor::getOrZero(SensorType::Tps1));

		efiPrintf("ETB1 duty=%.2f",
			(float)engine->outputChannels.etb1DutyCycle);

		efiPrintf("ETB freq=%d",
			engineConfiguration->etbFreq);

		for (int i = 0; i < ETB_COUNT; i++) {
			efiPrintf("ETB%d", i);
			efiPrintf(" dir1=%s", hwPortname(engineConfiguration->etbIo[i].directionPin1));
			efiPrintf(" dir2=%s", hwPortname(engineConfiguration->etbIo[i].directionPin2));
			efiPrintf(" control=%s", hwPortname(engineConfiguration->etbIo[i].controlPin));
			efiPrintf(" disable=%s", hwPortname(engineConfiguration->etbIo[i].disablePin));
			showDcMotorInfo(i);
		}
	});

#endif /* EFI_PROD_CODE */

	pedal2tpsMap.initTable(config->pedalToTpsTable, config->pedalToTpsRpmBins, config->pedalToTpsPedalBins);
	throttle2TrimTable.initTable(config->throttle2TrimTable, config->throttle2TrimRpmBins, config->throttle2TrimTpsBins);
	tcEtbDropTable.initTable(engineConfiguration->tractionControlEtbDrop, engineConfiguration->tractionControlSlipBins, engineConfiguration->tractionControlSpeedBins);

	doInitElectronicThrottle(/*isStartupInit*/true);
}

void setEtbIdlePosition(percent_t pos) {
	for (int i = 0; i < ETB_COUNT; i++) {
		if (auto etb = engine->etbControllers[i]) {
			assertNotNullVoid(etb);
			etb->setIdlePosition(pos);
		}
	}
}

void setEtbWastegatePosition(percent_t pos) {
	for (int i = 0; i < ETB_COUNT; i++) {
		if (auto etb = engine->etbControllers[i]) {
			assertNotNullVoid(etb);
			etb->setWastegatePosition(pos);
		}
	}
}

void setEtbLuaAdjustment(percent_t pos) {
	for (int i = 0; i < ETB_COUNT; i++) {
		/* TODO: use from engine, add getFunction() to base class */
		//if (auto etb = engine->etbControllers[i]) {
		if (auto etb = etbControllers[i]) {
			assertNotNullVoid(etb);
			// try to adjust all ETB
			if (etb->getFunction() == DC_Throttle1 || etb->getFunction() == DC_Throttle2) {
				etb->setLuaAdjustment(pos);
			}
		}
	}
}

void setEwgLuaAdjustment(percent_t pos) {
	for (int i = 0; i < ETB_COUNT; i++) {
		/* TODO: use from engine, add getFunction() to base class */
		//if (auto etb = engine->etbControllers[i]) {
		if (auto etb = etbControllers[i]) {
			assertNotNullVoid(etb);
			// try to adjust all ETB
			if (etb->getFunction() == DC_Wastegate) {
				etb->setLuaAdjustment(pos);
			}
		}
	}
}

void setToyota89281_33010_pedal_position_sensor() {
	setPPSCalibration(0, 4.1, 0.73, 4.9);
}

void setHitachiEtbCalibration() {
	setToyota89281_33010_pedal_position_sensor();

	setHitachiEtbBiasBins();

	engineConfiguration->etb.pFactor = 2.7999;
	engineConfiguration->etb.iFactor = 25.5;
	engineConfiguration->etb.dFactor = 0.053;
	engineConfiguration->etb.offset = 0.0;
	engineConfiguration->etb.periodMs = 5.0;
	engineConfiguration->etb.minValue = -100.0;
	engineConfiguration->etb.maxValue = 100.0;

	// Nissan 60mm throttle
	engineConfiguration->tpsMin = engineConfiguration->tps2Min = 113;
	engineConfiguration->tpsMax = engineConfiguration->tps2Max = 846;
	engineConfiguration->tps1SecondaryMin = engineConfiguration->tps2SecondaryMin = 897;
	engineConfiguration->tps1SecondaryMax = engineConfiguration->tps2SecondaryMax = 161;
}

void setProteusHitachiEtbDefaults() {
#if HW_PROTEUS
	setHitachiEtbCalibration();

	// EFI_ADC_12: "Analog Volt 3"
	engineConfiguration->tps1_2AdcChannel = PROTEUS_IN_TPS1_2;
	// EFI_ADC_13: "Analog Volt 4"
	engineConfiguration->tps2_1AdcChannel = PROTEUS_IN_TPS2_1;
	// EFI_ADC_0: "Analog Volt 5"
	engineConfiguration->tps2_2AdcChannel = PROTEUS_IN_ANALOG_VOLT_5;
	setPPSInputs(PROTEUS_IN_ANALOG_VOLT_6, PROTEUS_IN_PPS2);
#endif // HW_PROTEUS
}

#endif /* EFI_ELECTRONIC_THROTTLE_BODY */

// So far used by FragmentEntry (LiveData)
template<>
const electronic_throttle_s* getLiveData(size_t idx) {
#if EFI_ELECTRONIC_THROTTLE_BODY
	if (idx >= efi::size(etbControllers)) {
		return nullptr;
	}

	return etbControllers[idx];
#else
	return nullptr;
#endif
}

// root cause: we have poor usability around DC function and h-bridge stepper
void pickEtbOrStepper() {
  if (!engineConfiguration->useHbridgesToDriveIdleStepper) {
    return;
  }
  for (size_t i = 0;i<ETB_COUNT;i++) {
	  if (engineConfiguration->etbFunctions[i] != DC_None) {
      criticalError("Cannot use H-bridge for stepper while DC function is selected");
	  }
	}
}
